#include <ros/ros.h> 
#include <pcl/point_cloud.h> 
#include <pcl_conversions/pcl_conversions.h> 
#include <sensor_msgs/PointCloud2.h> 

struct Point
{
	double x;
	double y;
	double z;
	Point(double px,double py)
	{
		x = px;
		y = py;	
		z=0;
	}	
};
//draw bspline
/*
 * n  kongzhidianzhijianchazhidegeshu
 * points dangqiankongzhidian
 * return vec
 * */
bool bspline(int n,std::vector<Point>& p_in,std::vector<Point>& p_out)
{
	if(p_in.size()<4)
	{
		return false;
	}
	p_out.clear();
	float f1,f2,f3,f4;
	float deltaT = 1.0 /n;
	float T;
	for(int i=0;i<p_in.size()-3;i++)
	{
		for(int j=0;j<=n;j++)	
		{
			T = j * deltaT;
		    f1 = (-T*T*T + 3*T*T - 3*T + 1) / 6.0;
		    f2 = (3*T*T*T - 6*T*T + 4) / 6.0;
		    f3 = (-3*T*T*T +3*T*T + 3*T + 1) / 6.0;
		    f4 = (T*T*T) / 6.0;
			double x=f1*p_in[i].x+f2*p_in[i+1].x+f3*p_in[i+2].x+f4*p_in[i+3].x;
			double y=f1*p_in[i].y+f2*p_in[i+1].y+f3*p_in[i+2].y+f4*p_in[i+3].y;
//			std::cout<<"j: "<<j<<" x : "<<x<<" y: "<<y<<std::endl;
			Point  p(x,y);
			p_out.push_back(p);
		}
	}
	return true;
}
int main (int argc, char **argv) 
{ 
	ros::init (argc, argv, "pcl_create"); 
	
	ros::NodeHandle nh; 
	ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("point", 10000);     
	pcl::PointCloud<pcl::PointXYZ> cloud; 
	sensor_msgs::PointCloud2 output; 
	std::vector<Point> p_in,p_out;

	Point p0(0,0);
	p_in.push_back(p0);

	Point p1(1,1);
	p_in.push_back(p1);

	Point p2(0,2);
	p_in.push_back(p2);

	Point p3(1,3);
	p_in.push_back(p3);

	Point p4(1,4);
	p_in.push_back(p4);

	Point p5(0,6);
	p_in.push_back(p5);

	Point p6(1,8);
	p_in.push_back(p6);

	bspline(40,p_in,p_out);
	
	cloud.width=p_in.size()+p_out.size();
	cloud.height = 1; 
	cloud.points.resize(cloud.width * cloud.height); 
	cloud.points.clear();
	for (size_t i = 0; i < p_in.size (); ++i)
	{
		pcl::PointXYZ p;
		p.x=p_in[i].x;
		p.y=p_in[i].y;
		p.z=p_in[i].z;
		cloud.points.push_back(p);
	}
	for (size_t i = 0; i < p_out.size (); ++i)
	 { 
		pcl::PointXYZ p;
		p.x=p_out[i].x; 
		p.y=p_out[i].y; 
		p.z=p_out[i].z; 
		cloud.points.push_back(p);
 	} 
	
	//Convert the cloud to ROS message 
	pcl::toROSMsg(cloud, output); 
	output.header.frame_id = "odom"; 
	
	ros::Rate loop_rate(1); 
	while (ros::ok()) 
	{ 
		pcl_pub.publish(output);
	    ros::spinOnce(); 
		loop_rate.sleep(); 
	 } 
	return 0; 
}
